package com.ai.somatosensoryControl;

import org.opencv.core.*;
import org.opencv.dnn.Dnn;
import org.opencv.dnn.Net;
import org.opencv.highgui.HighGui;
import org.opencv.imgproc.Imgproc;
import org.opencv.videoio.VideoCapture;
import org.opencv.videoio.Videoio;

import java.io.File;
import java.util.ArrayList;
import java.util.List;

/**
 * 肢体姿态检测控制机器人移动
 */
public class PoseControlSystem {
    private static final int IN_WIDTH = 1208; //640; // 输入图像宽度
    private static final int IN_HEIGHT = 720; //480;   // 输入图像高度
    private static final double THRESHOLD = 0.57; // 阈值

    private Net net; // 网络


    /**
     * 初始化OpenCV库
     */
    static {
        String opencvDll = "D:/InstallationenvironmentHwp/jdk17/bin/opencv_java460.dll"; // opencv_java455.dll
        try {
            System.load(opencvDll);
        } catch (UnsatisfiedLinkError e) {
            throw new RuntimeException("OpenCV DLL加载失败: " + opencvDll, e);
        }
        if (Core.getVersionString() == null) {
            throw new RuntimeException("OpenCV初始化失败");
        }
        System.out.println("OpenCV初始化成功: " + Core.getVersionString());
    }

    /**
     * 初始化PoseControlSystem对象，并加载模型文件。
     * @param modelPath 模型文件路径
     * @param configPath  模型配置文件路径
     */
    public PoseControlSystem(String modelPath, String configPath) {
        validateModelFiles(modelPath, configPath);
        net = Dnn.readNetFromCaffe(configPath, modelPath);
        if (net.empty()) {
            throw new RuntimeException("加载模型失败");
        }
        configureNet();
    }

    /**
     * 验证模型文件是否存在
     * @param modelPath  模型文件路径
     * @param configPath  模型配置文件路径
     */
    private void validateModelFiles(String modelPath, String configPath) {
        if (!new File(modelPath).exists() || !new File(configPath).exists()) {
            throw new RuntimeException("找不到模型文件:\n" +
                    "模型: " + modelPath + "\n配置: " + configPath);
        }
    }

    /**
     * 配置网络，设置后端和后端目标, 后端和CPU为目标。后续可以改为GPU
     */
    private void configureNet() {
        net.setPreferableBackend(Dnn.DNN_BACKEND_OPENCV);
        net.setPreferableTarget(Dnn.DNN_TARGET_CPU);
    }

    /**
     *  检测人体姿态并返回关键点:
     *       在 detectPose 中，神经网络输出一个 Mat 对象，该对象包含了每个关键点的概率图。然后，代码会根据每个关键点的概率图提取出位置坐标，转化为 Point 对象并返回。每个 Point 包含该关键点在图像中的 (x, y) 坐标
     * @param frame
     * @return
     */
    public List<Point> detectPose(Mat frame) {
        Mat blob = Dnn.blobFromImage(frame, 1.0 / 255, new Size(IN_WIDTH, IN_HEIGHT),
                new Scalar(0, 0, 0), false, false);
        net.setInput(blob);
        Mat output = net.forward();
        blob.release();

        List<Point> points = processOutput(output, frame.size());
        output.release();
        return points;
    }

    /**
     * 处理输出，返回关键点
     * @param output  输出
     * @param frameSize  帧大小
     * @return
     */
    private List<Point> processOutput(Mat output, Size frameSize) {
        List<Point> points = new ArrayList<>();
        int H = output.size(2); // 高度
        int W = output.size(3); // 宽度

        for (int i = 0; i < 18; i++) {
            if (i < output.size(1)) {
                Mat probMap = output.submat(0, output.size(0), i, i + 1).reshape(1, H);
                Core.MinMaxLocResult mm = Core.minMaxLoc(probMap);
                probMap.release();

                points.add(mm.maxVal > THRESHOLD ? scalePoint(mm.maxLoc, W, H, frameSize) : null);
            }
        }
        return points;
    }

    /**
     * 将点从网络输出的尺寸缩放为实际尺寸
     * @param maxLoc 最大值位置
     * @param W  宽度
     * @param H  高度
     * @param frameSize  帧大小
     * @return
     */
    private Point scalePoint(Point maxLoc, int W, int H, Size frameSize) {
        return new Point(maxLoc.x * frameSize.width / W, maxLoc.y * frameSize.height / H);
    }

    /**
     * 控制机器人的移动，根据检测到的人体姿态进行控制。
     * @param points  关键点数据集
     */
    public void controlBasedOnPose(List<Point> points, Size frameSize) {
        if (points.size() >= 18) {
            // 获取每个关键点并添加null检查
            Point nose = points.get(0);        // 头部
            Point leftShoulder = points.get(5); // 左肩
            Point rightShoulder = points.get(6); // 右肩
            Point leftElbow = points.get(7);   // 左肘
            Point rightElbow = points.get(8);  // 右肘
            Point leftWrist = points.get(9);   // 左手腕
            Point rightWrist = points.get(10); // 右手腕
            Point leftHip = points.get(11);    // 左髋
            Point rightHip = points.get(12);   // 右髋
            Point leftKnee = points.get(13);   // 左膝
            Point rightKnee = points.get(14);  // 右膝
            Point leftAnkle = points.get(15);  // 左脚踝
            Point rightAnkle = points.get(16); // 右脚踝
            Point leftToe = points.get(17);    // 左脚尖

            // 控制逻辑：打印出每个部位的坐标（只有在该部位不为null时才打印）
            if (nose != null) {
                System.out.printf("头部(%.1f,%.1f) ", nose.x, nose.y);
            }
//            if (leftShoulder != null && rightShoulder != null) {
//                System.out.printf("左肩(%.1f,%.1f), 右肩(%.1f,%.1f) ", leftShoulder.x, leftShoulder.y, rightShoulder.x, rightShoulder.y);
//            }
//            if (leftElbow != null && rightElbow != null) {
//                System.out.printf("左肘(%.1f,%.1f), 右肘(%.1f,%.1f) ", leftElbow.x, leftElbow.y, rightElbow.x, rightElbow.y);
//            }
//            if (leftWrist != null && rightWrist != null) {
//                System.out.printf("左手腕(%.1f,%.1f), 右手腕(%.1f,%.1f) ", leftWrist.x, leftWrist.y, rightWrist.x, rightWrist.y);
//            }
//            if (leftHip != null && rightHip != null) {
//                System.out.printf("左髋(%.1f,%.1f), 右髋(%.1f,%.1f) ", leftHip.x, leftHip.y, rightHip.x, rightHip.y);
//            }
//            if (leftKnee != null && rightKnee != null) {
//                System.out.printf("左膝(%.1f,%.1f), 右膝(%.1f,%.1f) ", leftKnee.x, leftKnee.y, rightKnee.x, rightKnee.y);
//            }
//            if (leftAnkle != null && rightAnkle != null) {
//                System.out.printf("左脚踝(%.1f,%.1f), 右脚踝(%.1f,%.1f) ", leftAnkle.x, leftAnkle.y, rightAnkle.x, rightAnkle.y);
//            }
//            if (leftToe != null) {
//                System.out.printf("左脚尖(%.1f,%.1f)\n", leftToe.x, leftToe.y);
//            }

            // 检测并收集动作
            List<String> actions = detectAndPrintActions(
                    leftShoulder, rightShoulder, leftElbow, rightElbow,
                    leftWrist, rightWrist, leftKnee, rightKnee,
                    leftAnkle, rightAnkle, nose, leftHip, rightHip,frameSize);

            // 有动作时输出
            if (!actions.isEmpty()) {
                System.out.println("检测到动作: " + String.join(", ", actions));
            }

        }
    }

    /**
     * 处理动作
     * @param leftShoulder  左肩
     * @param rightShoulder  右肩
     * @param leftElbow  左肘
     * @param rightElbow  右肘
     * @param leftWrist  左手腕
     * @param rightWrist  右手腕
     * @param leftKnee  左膝盖
     * @param rightKnee  右膝盖
     * @param leftAnkle  左脚踝
     * @param rightAnkle  右脚踝
     * @param nose  头部
     * @param leftHip  左髋
     * @param rightHip  右髋
     */
    private List<String> detectAndPrintActions(Point leftShoulder, Point rightShoulder, Point leftElbow, Point rightElbow,
                                               Point leftWrist, Point rightWrist, Point leftKnee, Point rightKnee,
                                               Point leftAnkle, Point rightAnkle, Point nose, Point leftHip, Point rightHip, Size frameSize) {

        List<String> actions = new ArrayList<>();

        // 头部动作
        actions.addAll(handleHeadActions(nose, frameSize));

        // 上半身动作
        actions.addAll(handleUpperBodyActions(leftShoulder, rightShoulder, leftElbow, rightElbow, leftWrist, rightWrist, frameSize));

        // 下半身动作
        actions.addAll(handleLowerBodyActions(leftKnee, rightKnee, leftAnkle, rightAnkle, frameSize));

        // 全身动作
        actions.addAll(handleFullBodyActions(leftShoulder, rightShoulder, leftHip, rightHip, frameSize));

        return actions;

    }

    /**
     * 处理上半身动作
     * @param leftShoulder 左肩
     * @param rightShoulder 右肩
     * @param leftElbow 左肘
     * @param rightElbow 右肘
     * @param leftWrist 左手腕
     * @param rightWrist 右手腕
     */
    private List<String> handleUpperBodyActions(Point leftShoulder, Point rightShoulder, Point leftElbow, Point rightElbow,
                                                Point leftWrist, Point rightWrist, Size frameSize) {
        List<String> actions = new ArrayList<>();
        if (leftShoulder != null && rightShoulder != null) {
            double shoulderDistance = Math.sqrt(Math.pow(leftShoulder.x - rightShoulder.x, 2) +
                    Math.pow(leftShoulder.y - rightShoulder.y, 2));
            double distanceThreshold = frameSize.width * 0.15; // 使用宽度比例
            if (shoulderDistance < distanceThreshold) {
                actions.add("双臂收拢");
            }

            // 更精确的举手判断（示例：手腕在肩膀上方）
            if (leftWrist != null && leftWrist.y < leftShoulder.y) {
                actions.add("左臂举起");
            }
            if (rightWrist != null && rightWrist.y < rightShoulder.y) {
                actions.add("右臂举起");
            }
        }
        return actions;
    }

    /**
     * 处理下半身动作
     * @param leftKnee 左膝盖
     * @param rightKnee 右膝盖
     * @param leftAnkle 左脚踝
     * @param rightAnkle 右脚踝
     */
    private List<String> handleLowerBodyActions(Point leftKnee, Point rightKnee, Point leftAnkle, Point rightAnkle,Size frameSize) {
        List<String> actions = new ArrayList<>();
        if (leftKnee != null && rightKnee != null) {
            double kneeAngle = Math.abs(leftKnee.y - rightKnee.y);
            double distanceThreshold = frameSize.width * 0.15; // 使用宽度比例
            if (kneeAngle < distanceThreshold ) { // 50
                actions.add("站立");
            } else if (kneeAngle > distanceThreshold ) { // 150
                actions.add("蹲下");
            }
        }

        if (leftAnkle != null && rightAnkle != null) {
            double ankleDistance = Math.abs(leftAnkle.x - rightAnkle.x);
            double distanceThreshold = frameSize.width * 0.15; // 使用宽度比例
            if (ankleDistance > distanceThreshold) { // 200
                actions.add("行走");
            } else if (ankleDistance > distanceThreshold) { //300
                actions.add("跑步");
            }
        }
        return actions;
    }

    /**
     * 处理头部动作
     * @param nose 头部
     */
    private List<String> handleHeadActions(Point nose, Size frameSize) {
        List<String> actions = new ArrayList<>();
        if (nose != null) {
            // 使用相对位置判断（示例：头部在图像上半部分）
            if (nose.y < frameSize.height * 0.3) {
                actions.add("头部点头");
            } else if (nose.x < frameSize.width * 0.2) {
                actions.add("头部左转");
            } else if (nose.x > frameSize.width * 0.8 ) {
                actions.add("头部右转");
            }
        }
        return actions;
    }

    /**
     * 处理全身动作
     * @param leftShoulder  左肩
     * @param rightShoulder 右肩
     * @param leftHip  左髋
     * @param rightHip  右髋
     */
    private List<String> handleFullBodyActions(Point leftShoulder, Point rightShoulder, Point leftHip, Point rightHip, Size frameSize) {
        List<String> actions = new ArrayList<>();
        if (leftShoulder != null && rightShoulder != null && leftHip != null && rightHip != null) {
            double shoulderAngle = Math.abs(leftShoulder.x - rightShoulder.x);
            double hipAngle = Math.abs(leftHip.x - rightHip.x);
            double distanceThreshold = frameSize.width * 0.15; // 使用宽度比例
            if (shoulderAngle > distanceThreshold && hipAngle < distanceThreshold) { // > 150   && < 100
                actions.add("转身");
            }
        }
        return actions;
    }

    /**
     * 释放资源
     */
    public void close() {
        if (net != null) {
            // net.release();  // 如果不再需要，释放网络资源
        }
    }

    /**
     * 主函数
     * @param args
     */
    public static void main(String[] args) {

        System.setProperty("OPENCV_VIDEOIO_PRIORITY_MSMF", "1"); // 优先使用Media Foundation
        System.setProperty("OPENCV_LOG_LEVEL", "ERROR"); // 日志级别设为ERROR

        try {

            // 创建PoseControlSystem对象, 传入模型文件和配置文件(常见的 OpenPose 模型)
            PoseControlSystem poseControl = new PoseControlSystem(
                    "D:\\VR\\opencv\\openpose_caffe_models\\caffe_models\\pose\\coco\\pose_iter_440000.caffemodel",
                    "D:\\VR\\opencv\\openpose_caffe_models\\caffe_models\\pose\\coco\\pose_deploy_linevec.prototxt");

            // 打开摄像头
            VideoCapture capture = openCamera();

            // 创建Mat对象
            Mat frame = new Mat();

            // 循环处理视频流
            while (true) {
                // 捕获一帧视频
                if (!capture.read(frame)) {
                    System.err.println("帧捕获失败");
                    break;
                }
                // 检查帧是否为空
                if (frame.empty()) continue;

                // 处理帧
                processFrame(poseControl, frame);
            }
            // 释放资源
            releaseResources(capture, poseControl, frame);
        } catch (Exception e) {
            e.printStackTrace();
        }
    }

    /**
     * 处理帧
     * @param poseControl PoseControlSystem对象
     * @param frame Mat对象
     */
    private static void processFrame(PoseControlSystem poseControl, Mat frame) {
        if (frame.empty()) {
            System.err.println("捕获的帧为空");
            return;
        }

        Mat displayFrame = new Mat();// 创建一个与输入Mat对象大小相同的Mat对象
        try {
            frame.copyTo(displayFrame); // 复制输入Mat对象到输出Mat对象
            List<Point> points = poseControl.detectPose(displayFrame); // 检测关键点
            poseControl.controlBasedOnPose(points, displayFrame.size()); // 传递帧尺寸
            drawLandmarks(displayFrame, points); // 绘制关键点

            if (displayFrame.size().width > 0 && displayFrame.size().height > 0) { // 确保Mat对象不为空
                HighGui.imshow("姿态控制", displayFrame);
            }

        } finally {
            if (!displayFrame.empty()) {
                displayFrame.release(); // 确保释放资源
            }
        }
    }

    /**
     * 绘制关键点
     * @param frame Mat对象
     * @param points 关键点列表
     */
    private static void drawLandmarks(Mat frame, List<Point> points) {
        for (Point p : points) {
            if (p != null) {
                Imgproc.circle(frame, p, 5, new Scalar(0, 255, 0), -1);
            }
        }
    }

    /**
     * 打开摄像头
     * @return
     */
    private static VideoCapture openCamera() {
        VideoCapture capture = new VideoCapture();
        if (!capture.open(0, Videoio.CAP_MSMF)) {
            if (!capture.open(0, Videoio.CAP_DSHOW)) {
                if (!capture.open(0, Videoio.CAP_ANY)) {
                    throw new RuntimeException("无法打开摄像头");
                }
            }
        }
        capture.set(Videoio.CAP_PROP_FRAME_WIDTH, 1280);    // 设置分辨率
        capture.set(Videoio.CAP_PROP_FRAME_HEIGHT, 720); // 设置分辨率
        capture.set(Videoio.CAP_PROP_FPS, 30); // 设置帧率
        capture.set(Videoio.CAP_PROP_AUTOFOCUS, 0); // 关闭自动对焦
        return capture;
    }

    /**
     * 释放资源
     * @param capture  视频捕获对象
     * @param poseControl  PoseControlSystem对象
     * @param frame  Mat对象
     */
    private static void releaseResources(VideoCapture capture, PoseControlSystem poseControl, Mat frame) {
        capture.release();
        poseControl.close();
        frame.release();
        HighGui.destroyAllWindows();
    }
}



//package com.ai.somatosensoryControl;
//
//import org.opencv.core.*;
//import org.opencv.dnn.Dnn;
//import org.opencv.dnn.Net;
//import org.opencv.highgui.HighGui;
//import org.opencv.imgproc.Imgproc;
//import org.opencv.videoio.VideoCapture;
//import org.opencv.videoio.Videoio;
//
//import java.io.File;
//import java.util.ArrayList;
//import java.util.List;
//
///**
// * 肢体姿态检测控制机器人移动
// */
//public class PoseControlSystem {
//    // 模型输入尺寸（调整为标准OpenPose尺寸）
//    private static final int IN_WIDTH = 656;
//    private static final int IN_HEIGHT = 368;
//
//    // 检测参数（提高置信度阈值）
//    private static final double THRESHOLD = 0.65;
//
//    // 动作判断阈值（基于图像尺寸的比例）
//    private static final double SHOULDER_CLOSE_RATIO = 0.12; // 值越小对收拢动作要求越严格
//    private static final double ARM_RAISE_RATIO = 0.25;     // 值越大需要举得越高
//    private static final double KNEE_BEND_RATIO = 0.15;     // 值越大对下蹲要求越高
//    private static final double HEAD_TURN_RATIO = 0.25;     // 值越小对头部转动越敏感
//    private static final double ANKLE_MOVE_RATIO = 0.2;
//
//    private Net net;
//
//    static {
//        System.load("D:/InstallationenvironmentHwp/jdk17/bin/opencv_java460.dll");
//        if (Core.getVersionString() == null) {
//            throw new RuntimeException("OpenCV初始化失败");
//        }
//    }
//
//    /**
//     * 初始化PoseControlSystem对象，并加载模型文件。
//     * @param modelPath 模型文件路径
//     * @param configPath  模型配置文件路径
//     */
//    public PoseControlSystem(String modelPath, String configPath) {
//        validateModelFiles(modelPath, configPath);
//        net = Dnn.readNetFromCaffe(configPath, modelPath);
//        if (net.empty()) throw new RuntimeException("加载模型失败");
//        configureNet();
//    }
//
//    /**
//     * 验证模型文件是否存在
//     * @param modelPath  模型文件路径
//     * @param configPath  模型配置文件路径
//     */
//    private void validateModelFiles(String modelPath, String configPath) {
//        if (!new File(modelPath).exists() || !new File(configPath).exists()) {
//            throw new RuntimeException("找不到模型文件:\n模型: " + modelPath + "\n配置: " + configPath);
//        }
//    }
//
//    /**
//     * 配置网络，设置后端和后端目标, 后端和CPU为目标。后续可以改为GPU
//     */
//    private void configureNet() {
//        net.setPreferableBackend(Dnn.DNN_BACKEND_OPENCV);
//        net.setPreferableTarget(Dnn.DNN_TARGET_CPU);
//    }
//
//    /**
//     *  检测人体姿态并返回关键点:
//     *       在 detectPose 中，神经网络输出一个 Mat 对象，该对象包含了每个关键点的概率图。然后，代码会根据每个关键点的概率图提取出位置坐标，转化为 Point 对象并返回。每个 Point 包含该关键点在图像中的 (x, y) 坐标
//     * @param frame
//     * @return
//     */
//    public List<Point> detectPose(Mat frame) {
//        Mat blob = Dnn.blobFromImage(frame, 1.0 / 255, new Size(IN_WIDTH, IN_HEIGHT),
//                new Scalar(0, 0, 0), false, false);
//        net.setInput(blob);
//        Mat output = net.forward();
//        blob.release();
//
//        List<Point> points = processOutput(output, frame.size());
//        output.release();
//        return points;
//    }
//
//    /**
//     * 处理输出，返回关键点
//     * @param output  输出
//     * @param frameSize  帧大小
//     * @return
//     */
//    private List<Point> processOutput(Mat output, Size frameSize) {
//        List<Point> points = new ArrayList<>();
//        int H = output.size(2);
//        int W = output.size(3);
//
//        for (int i = 0; i < 18; i++) {
//            if (i < output.size(1)) {
//                Mat probMap = output.submat(0, output.size(0), i, i + 1).reshape(1, H);
//                Core.MinMaxLocResult mm = Core.minMaxLoc(probMap);
//                probMap.release();
//
//                points.add(mm.maxVal > THRESHOLD ? scalePoint(mm.maxLoc, W, H, frameSize) : null);
//            }
//        }
//        return points;
//    }
//
//    /**
//     * 将点从网络输出的尺寸缩放为实际尺寸
//     * @param maxLoc 最大值位置
//     * @param W  宽度
//     * @param H  高度
//     * @param frameSize  帧大小
//     * @return
//     */
//    private Point scalePoint(Point maxLoc, int W, int H, Size frameSize) {
//        return new Point(maxLoc.x * frameSize.width / W, maxLoc.y * frameSize.height / H);
//    }
//
//    /**
//     * 控制机器人的移动，根据检测到的人体姿态进行控制。
//     * @param points  关键点数据集
//     */
//    public void controlBasedOnPose(List<Point> points, Size frameSize) {
//        if (points.size() >= 18) {
//            // 获取每个关键点并添加null检查
//            Point nose = points.get(0);        // 头部
//            Point leftShoulder = points.get(5); // 左肩
//            Point rightShoulder = points.get(6); // 右肩
//            Point leftElbow = points.get(7);   // 左肘
//            Point rightElbow = points.get(8);  // 右肘
//            Point leftWrist = points.get(9);   // 左手腕
//            Point rightWrist = points.get(10); // 右手腕
//            Point leftHip = points.get(11);    // 左髋
//            Point rightHip = points.get(12);   // 右髋
//            Point leftKnee = points.get(13);   // 左膝
//            Point rightKnee = points.get(14);  // 右膝
//            Point leftAnkle = points.get(15);  // 左脚踝
//            Point rightAnkle = points.get(16); // 右脚踝
//            Point leftToe = points.get(17);    // 左脚尖
//
//            // 控制逻辑：打印出每个部位的坐标（只有在该部位不为null时才打印）
//            if (nose != null) {
//                System.out.printf("头部(%.1f,%.1f) ", nose.x, nose.y);
//            }
//            if (leftShoulder != null && rightShoulder != null) {
//                System.out.printf("左肩(%.1f,%.1f), 右肩(%.1f,%.1f) ", leftShoulder.x, leftShoulder.y, rightShoulder.x, rightShoulder.y);
//            }
//            if (leftElbow != null && rightElbow != null) {
//                System.out.printf("左肘(%.1f,%.1f), 右肘(%.1f,%.1f) ", leftElbow.x, leftElbow.y, rightElbow.x, rightElbow.y);
//            }
//            if (leftWrist != null && rightWrist != null) {
//                System.out.printf("左手腕(%.1f,%.1f), 右手腕(%.1f,%.1f) ", leftWrist.x, leftWrist.y, rightWrist.x, rightWrist.y);
//            }
//            if (leftHip != null && rightHip != null) {
//                System.out.printf("左髋(%.1f,%.1f), 右髋(%.1f,%.1f) ", leftHip.x, leftHip.y, rightHip.x, rightHip.y);
//            }
//            if (leftKnee != null && rightKnee != null) {
//                System.out.printf("左膝(%.1f,%.1f), 右膝(%.1f,%.1f) ", leftKnee.x, leftKnee.y, rightKnee.x, rightKnee.y);
//            }
//            if (leftAnkle != null && rightAnkle != null) {
//                System.out.printf("左脚踝(%.1f,%.1f), 右脚踝(%.1f,%.1f) ", leftAnkle.x, leftAnkle.y, rightAnkle.x, rightAnkle.y);
//            }
//            if (leftToe != null) {
//                System.out.printf("左脚尖(%.1f,%.1f)\n", leftToe.x, leftToe.y);
//            }
//
//
//            List<String> actions = detectActions(
//                    points.get(5),  // 左肩
//                    points.get(6),  // 右肩
//                    points.get(7),  // 左肘
//                    points.get(8),  // 右肘
//                    points.get(9),  // 左手腕
//                    points.get(10), // 右手腕
//                    points.get(13), // 左膝
//                    points.get(14), // 右膝
//                    points.get(15), // 左脚踝
//                    points.get(16), // 右脚踝
//                    points.get(0),  // 鼻子
//                    points.get(11), // 左髋
//                    points.get(12), // 右髋
//                    frameSize
//            );
//
//
//
//
//            if (!actions.isEmpty()) {
//                System.out.println("检测到动作: " + String.join(", ", actions));
//            }
//        }
//    }
//
//
//
//
//
//
//    private List<String> detectActions(Point ls, Point rs, Point le, Point re,
//                                       Point lw, Point rw, Point lk, Point rk,
//                                       Point la, Point ra, Point nose,
//                                       Point lh, Point rh, Size frameSize) {
//        List<String> actions = new ArrayList<>();
//
//        // 双臂动作检测
//        if (ls != null && rs != null) {
//            double shoulderDist = Math.hypot(ls.x - rs.x, ls.y - rs.y);
//            double closeThreshold = frameSize.width * SHOULDER_CLOSE_RATIO;
//
//            if (shoulderDist < closeThreshold) {
//                actions.add("双臂收拢");
//            }
//        }
//
//        // 单臂举起检测（需要完整的胳膊关键点）
//        if (ls != null && le != null && lw != null) {
//            if (isArmRaised(ls, le, lw, frameSize)) {
//                actions.add("左臂举起");
//            }
//        }
//        if (rs != null && re != null && rw != null) {
//            if (isArmRaised(rs, re, rw, frameSize)) {
//                actions.add("右臂举起");
//            }
//        }
//
//        // 下半身动作检测
//        if (lk != null && rk != null) {
//            double kneeVertDist = Math.abs(lk.y - rk.y);
//            double threshold = frameSize.height * KNEE_BEND_RATIO;
//
//            if (kneeVertDist > threshold) {
//                actions.add("蹲下");
//            }
//        }
//
//        // 头部动作检测
//        if (nose != null) {
//            if (nose.x < frameSize.width * HEAD_TURN_RATIO) {
//                actions.add("头部左转");
//            } else if (nose.x > frameSize.width * (1 - HEAD_TURN_RATIO)) {
//                actions.add("头部右转");
//            }
//        }
//
//        // 全身转身检测
//        if (ls != null && rs != null && lh != null && rh != null) {
//            double shoulderWidth = Math.hypot(ls.x - rs.x, ls.y - rs.y);
//            double hipWidth = Math.hypot(lh.x - rh.x, lh.y - rh.y);
//
//            if (shoulderWidth > hipWidth * 1.5) {
//                actions.add("转身");
//            }
//        }
//
//        return actions;
//    }
//
//    private boolean isArmRaised(Point shoulder, Point elbow, Point wrist, Size frameSize) {
//        // 手臂垂直方向移动检测
//        double armLength = Math.hypot(shoulder.x - wrist.x, shoulder.y - wrist.y);
//        double verticalMove = shoulder.y - wrist.y;
//
//        // 需要满足两个条件：
//        // 1. 手腕在肩膀上方一定距离
//        // 2. 手臂基本伸直（肘关节角度）
//        return (verticalMove > frameSize.height * ARM_RAISE_RATIO) &&
//                (armLength > frameSize.height * 0.4);
//    }
//
//    /**
//     * 释放资源
//     */
//    public void close() {
//        if (net != null) {
//            // net.release();  // 如果不再需要，释放网络资源
//        }
//    }
//
//    /**
//     * 主函数
//     * @param args
//     */
//    public static void main(String[] args) {
//
//        System.setProperty("OPENCV_VIDEOIO_PRIORITY_MSMF", "1"); // 优先使用Media Foundation
//        System.setProperty("OPENCV_LOG_LEVEL", "ERROR"); // 日志级别设为ERROR
//
//        try {
//
//            // 创建PoseControlSystem对象, 传入模型文件和配置文件(常见的 OpenPose 模型)
//            PoseControlSystem poseControl = new PoseControlSystem(
//                    "D:\\VR\\opencv\\openpose_caffe_models\\caffe_models\\pose\\coco\\pose_iter_440000.caffemodel",
//                    "D:\\VR\\opencv\\openpose_caffe_models\\caffe_models\\pose\\coco\\pose_deploy_linevec.prototxt");
//
//            // 打开摄像头
//            VideoCapture capture = openCamera();
//
//            // 创建Mat对象
//            Mat frame = new Mat();
//
//            // 循环处理视频流
//            while (true) {
//                // 捕获一帧视频
//                if (!capture.read(frame)) {
//                    System.err.println("帧捕获失败");
//                    break;
//                }
//                // 检查帧是否为空
//                if (frame.empty()) continue;
//
//                // 处理帧
//                processFrame(poseControl, frame);
//            }
//            // 释放资源
//            releaseResources(capture, poseControl, frame);
//        } catch (Exception e) {
//            e.printStackTrace();
//        }
//    }
//
//    /**
//     * 处理帧
//     * @param poseControl PoseControlSystem对象
//     * @param frame Mat对象
//     */
//    private static void processFrame(PoseControlSystem poseControl, Mat frame) {
//        Mat displayFrame = new Mat();
//        try {
//            frame.copyTo(displayFrame);
//            List<Point> points = poseControl.detectPose(displayFrame);
//            poseControl.controlBasedOnPose(points, displayFrame.size());
//            drawLandmarks(displayFrame, points);
//
//            if (!displayFrame.empty()) {
//                HighGui.imshow("姿态控制", displayFrame);
//            }
//        } finally {
//            displayFrame.release();
//        }
//    }
//
//    /**
//     * 绘制关键点
//     * @param frame Mat对象
//     * @param points 关键点列表
//     */
//    private static void drawLandmarks(Mat frame, List<Point> points) {
//        for (Point p : points) {
//            if (p != null) {
//                Imgproc.circle(frame, p, 5, new Scalar(0, 255, 0), -1);
//            }
//        }
//    }
//
//    /**
//     * 打开摄像头
//     * @return
//     */
//    private static VideoCapture openCamera() {
//        VideoCapture capture = new VideoCapture();
//        if (!capture.open(0, Videoio.CAP_MSMF)) {
//            if (!capture.open(0, Videoio.CAP_DSHOW)) {
//                if (!capture.open(0, Videoio.CAP_ANY)) {
//                    throw new RuntimeException("无法打开摄像头");
//                }
//            }
//        }
//        capture.set(Videoio.CAP_PROP_FRAME_WIDTH, 1280);    // 设置分辨率
//        capture.set(Videoio.CAP_PROP_FRAME_HEIGHT, 720); // 设置分辨率
//        capture.set(Videoio.CAP_PROP_FPS, 30); // 设置帧率
//        capture.set(Videoio.CAP_PROP_AUTOFOCUS, 0); // 关闭自动对焦
//        return capture;
//    }
//
//    /**
//     * 释放资源
//     * @param capture  视频捕获对象
//     * @param poseControl  PoseControlSystem对象
//     * @param frame  Mat对象
//     */
//    private static void releaseResources(VideoCapture capture, PoseControlSystem poseControl, Mat frame) {
//        capture.release();
//        poseControl.close();
//        frame.release();
//        HighGui.destroyAllWindows();
//    }
//}





























//package com.ai.somatosensoryControl;
//
//import org.opencv.core.*;
//import org.opencv.dnn.Dnn;
//import org.opencv.dnn.Net;
//import org.opencv.highgui.HighGui;
//import org.opencv.imgproc.Imgproc;
//import org.opencv.videoio.VideoCapture;
//import org.opencv.videoio.Videoio;
//
//import java.io.File;
//import java.util.ArrayList;
//import java.util.List;
//
///**
// * 肢体姿态检测控制机器人移动
// */
//public class PoseControlSystem {
//    private static final int IN_WIDTH = 1208; //640; // 输入图像宽度
//    private static final int IN_HEIGHT = 720; //480;   // 输入图像高度
//    private static final double THRESHOLD = 0.3; // 阈值
//
//    private Net net; // 网络
//
//
//    /**
//     * 初始化OpenCV库
//     */
//    static {
//        String opencvDll = "D:/InstallationenvironmentHwp/jdk17/bin/opencv_java460.dll"; // opencv_java455.dll
//        try {
//            System.load(opencvDll);
//        } catch (UnsatisfiedLinkError e) {
//            throw new RuntimeException("OpenCV DLL加载失败: " + opencvDll, e);
//        }
//        if (Core.getVersionString() == null) {
//            throw new RuntimeException("OpenCV初始化失败");
//        }
//        System.out.println("OpenCV初始化成功: " + Core.getVersionString());
//    }
//
//    /**
//     * 初始化PoseControlSystem对象，并加载模型文件。
//     * @param modelPath 模型文件路径
//     * @param configPath  模型配置文件路径
//     */
//    public PoseControlSystem(String modelPath, String configPath) {
//        validateModelFiles(modelPath, configPath);
//        net = Dnn.readNetFromCaffe(configPath, modelPath);
//        if (net.empty()) {
//            throw new RuntimeException("加载模型失败");
//        }
//        configureNet();
//    }
//
//    /**
//     * 验证模型文件是否存在
//     * @param modelPath  模型文件路径
//     * @param configPath  模型配置文件路径
//     */
//    private void validateModelFiles(String modelPath, String configPath) {
//        if (!new File(modelPath).exists() || !new File(configPath).exists()) {
//            throw new RuntimeException("找不到模型文件:\n" +
//                    "模型: " + modelPath + "\n配置: " + configPath);
//        }
//    }
//
//    /**
//     * 配置网络，设置后端和后端目标, 后端和CPU为目标。后续可以改为GPU
//     */
//    private void configureNet() {
//        net.setPreferableBackend(Dnn.DNN_BACKEND_OPENCV);
//        net.setPreferableTarget(Dnn.DNN_TARGET_CPU);
//    }
//
//    /**
//     *  检测人体姿态并返回关键点:
//     *       在 detectPose 中，神经网络输出一个 Mat 对象，该对象包含了每个关键点的概率图。然后，代码会根据每个关键点的概率图提取出位置坐标，转化为 Point 对象并返回。每个 Point 包含该关键点在图像中的 (x, y) 坐标
//     * @param frame
//     * @return
//     */
//    public List<Point> detectPose(Mat frame) {
//        Mat blob = Dnn.blobFromImage(frame, 1.0 / 255, new Size(IN_WIDTH, IN_HEIGHT),
//                new Scalar(0, 0, 0), false, false);
//        net.setInput(blob);
//        Mat output = net.forward();
//        blob.release();
//
//        List<Point> points = processOutput(output, frame.size());
//        output.release();
//        return points;
//    }
//
//    /**
//     * 处理输出，返回关键点
//     * @param output  输出
//     * @param frameSize  帧大小
//     * @return
//     */
//    private List<Point> processOutput(Mat output, Size frameSize) {
//        List<Point> points = new ArrayList<>();
//        int H = output.size(2); // 高度
//        int W = output.size(3); // 宽度
//
//        for (int i = 0; i < 18; i++) {
//            if (i < output.size(1)) {
//                Mat probMap = output.submat(0, output.size(0), i, i + 1).reshape(1, H);
//                Core.MinMaxLocResult mm = Core.minMaxLoc(probMap);
//                probMap.release();
//
//                points.add(mm.maxVal > THRESHOLD ? scalePoint(mm.maxLoc, W, H, frameSize) : null);
//            }
//        }
//        return points;
//    }
//
//    /**
//     * 将点从网络输出的尺寸缩放为实际尺寸
//     * @param maxLoc 最大值位置
//     * @param W  宽度
//     * @param H  高度
//     * @param frameSize  帧大小
//     * @return
//     */
//    private Point scalePoint(Point maxLoc, int W, int H, Size frameSize) {
//        return new Point(maxLoc.x * frameSize.width / W, maxLoc.y * frameSize.height / H);
//    }
//
//    /**
//     * 控制机器人的移动，根据检测到的人体姿态进行控制。
//     * @param points  关键点数据集
//     */
//    public void controlBasedOnPose(List<Point> points) {
//        if (points.size() >= 18) {
//            // 获取每个关键点并添加null检查
//            Point nose = points.get(0);        // 头部
//            Point leftShoulder = points.get(5); // 左肩
//            Point rightShoulder = points.get(6); // 右肩
//            Point leftElbow = points.get(7);   // 左肘
//            Point rightElbow = points.get(8);  // 右肘
//            Point leftWrist = points.get(9);   // 左手腕
//            Point rightWrist = points.get(10); // 右手腕
//            Point leftHip = points.get(11);    // 左髋
//            Point rightHip = points.get(12);   // 右髋
//            Point leftKnee = points.get(13);   // 左膝
//            Point rightKnee = points.get(14);  // 右膝
//            Point leftAnkle = points.get(15);  // 左脚踝
//            Point rightAnkle = points.get(16); // 右脚踝
//            Point leftToe = points.get(17);    // 左脚尖
//
//            // 控制逻辑：打印出每个部位的坐标（只有在该部位不为null时才打印）
//            if (nose != null) {
//                System.out.printf("头部(%.1f,%.1f) ", nose.x, nose.y);
//            }
//            if (leftShoulder != null && rightShoulder != null) {
//                System.out.printf("左肩(%.1f,%.1f), 右肩(%.1f,%.1f) ", leftShoulder.x, leftShoulder.y, rightShoulder.x, rightShoulder.y);
//            }
//            if (leftElbow != null && rightElbow != null) {
//                System.out.printf("左肘(%.1f,%.1f), 右肘(%.1f,%.1f) ", leftElbow.x, leftElbow.y, rightElbow.x, rightElbow.y);
//            }
//            if (leftWrist != null && rightWrist != null) {
//                System.out.printf("左手腕(%.1f,%.1f), 右手腕(%.1f,%.1f) ", leftWrist.x, leftWrist.y, rightWrist.x, rightWrist.y);
//            }
//            if (leftHip != null && rightHip != null) {
//                System.out.printf("左髋(%.1f,%.1f), 右髋(%.1f,%.1f) ", leftHip.x, leftHip.y, rightHip.x, rightHip.y);
//            }
//            if (leftKnee != null && rightKnee != null) {
//                System.out.printf("左膝(%.1f,%.1f), 右膝(%.1f,%.1f) ", leftKnee.x, leftKnee.y, rightKnee.x, rightKnee.y);
//            }
//            if (leftAnkle != null && rightAnkle != null) {
//                System.out.printf("左脚踝(%.1f,%.1f), 右脚踝(%.1f,%.1f) ", leftAnkle.x, leftAnkle.y, rightAnkle.x, rightAnkle.y);
//            }
//            if (leftToe != null) {
//                System.out.printf("左脚尖(%.1f,%.1f)\n", leftToe.x, leftToe.y);
//            }
//
//            // 只在检测到特定动作时输出结果
//            //detectAndPrintActions(leftShoulder, rightShoulder, leftElbow, rightElbow, leftWrist, rightWrist,leftKnee, rightKnee, leftAnkle, rightAnkle, nose, leftHip, rightHip);
//        }
//    }
//
//    /**
//     * 处理动作
//     * @param leftShoulder  左肩
//     * @param rightShoulder  右肩
//     * @param leftElbow  左肘
//     * @param rightElbow  右肘
//     * @param leftWrist  左手腕
//     * @param rightWrist  右手腕
//     * @param leftKnee  左膝盖
//     * @param rightKnee  右膝盖
//     * @param leftAnkle  左脚踝
//     * @param rightAnkle  右脚踝
//     * @param nose  头部
//     * @param leftHip  左髋
//     * @param rightHip  右髋
//     */
//    private void detectAndPrintActions(Point leftShoulder, Point rightShoulder, Point leftElbow, Point rightElbow,
//                                       Point leftWrist, Point rightWrist, Point leftKnee, Point rightKnee,
//                                       Point leftAnkle, Point rightAnkle, Point nose, Point leftHip, Point rightHip) {
//
//        // 头部动作
//        handleHeadActions(nose);
//
//        // 上半身动作
//        handleUpperBodyActions(leftShoulder, rightShoulder, leftElbow, rightElbow, leftWrist, rightWrist);
//
//        // 下半身动作
//        handleLowerBodyActions(leftKnee, rightKnee, leftAnkle, rightAnkle);
//
//        // 全身动作
//        handleFullBodyActions(leftShoulder, rightShoulder, leftHip, rightHip);
//
//    }
//
//    /**
//     * 处理上半身动作
//     * @param leftShoulder 左肩
//     * @param rightShoulder 右肩
//     * @param leftElbow 左肘
//     * @param rightElbow 右肘
//     * @param leftWrist 左手腕
//     * @param rightWrist 右手腕
//     */
//    private void handleUpperBodyActions(Point leftShoulder, Point rightShoulder, Point leftElbow, Point rightElbow,
//                                        Point leftWrist, Point rightWrist) {
//        if (leftShoulder != null && rightShoulder != null) {
//            double shoulderDistance = Math.sqrt(Math.pow(leftShoulder.x - rightShoulder.x, 2) +
//                    Math.pow(leftShoulder.y - rightShoulder.y, 2));
//            if (shoulderDistance < 100 ) {
//                System.out.println("动作: 双臂收拢");
//            } else if (leftShoulder.y < rightShoulder.y ) {
//                System.out.println("动作: 双臂举起");
//            }
//        }
//
//        if (leftWrist != null && rightWrist != null) {
//            double wristDistance = Math.sqrt(Math.pow(leftWrist.x - rightWrist.x, 2) +
//                    Math.pow(leftWrist.y - rightWrist.y, 2));
//            if (wristDistance < 50) {
//                System.out.println("动作: 双手握拳");
//            }
//        }
//    }
//
//    /**
//     * 处理下半身动作
//     * @param leftKnee 左膝盖
//     * @param rightKnee 右膝盖
//     * @param leftAnkle 左脚踝
//     * @param rightAnkle 右脚踝
//     */
//    private void handleLowerBodyActions(Point leftKnee, Point rightKnee, Point leftAnkle, Point rightAnkle) {
//        if (leftKnee != null && rightKnee != null) {
//            double kneeAngle = Math.abs(leftKnee.y - rightKnee.y);
//            if (kneeAngle < 50 ) {
//                System.out.println("动作: 站立");
//            } else if (kneeAngle > 150 ) {
//                System.out.println("动作: 蹲下");
//            }
//        }
//
//        if (leftAnkle != null && rightAnkle != null) {
//            double ankleDistance = Math.abs(leftAnkle.x - rightAnkle.x);
//            if (ankleDistance > 200) {
//                System.out.println("动作: 行走");
//            } else if (ankleDistance > 300) {
//                System.out.println("动作: 跑步");
//            }
//        }
//    }
//
//    /**
//     * 处理头部动作
//     * @param nose 头部
//     */
//    private void handleHeadActions(Point nose) {
//        if (nose != null) {
//            if (nose.y < 200 ) {
//                System.out.println("动作: 头部点头");
//            } else if (nose.x < 100 ) {
//                System.out.println("动作: 头部左转");
//            } else if (nose.x > 500 ) {
//                System.out.println("动作: 头部右转");
//            }
//        }
//    }
//
//    /**
//     * 处理全身动作
//     * @param leftShoulder  左肩
//     * @param rightShoulder 右肩
//     * @param leftHip  左髋
//     * @param rightHip  右髋
//     */
//    private void handleFullBodyActions(Point leftShoulder, Point rightShoulder, Point leftHip, Point rightHip) {
//        if (leftShoulder != null && rightShoulder != null && leftHip != null && rightHip != null) {
//            double shoulderAngle = Math.abs(leftShoulder.x - rightShoulder.x);
//            double hipAngle = Math.abs(leftHip.x - rightHip.x);
//
//            if (shoulderAngle > 150 && hipAngle < 100) {
//                System.out.println("动作: 转身");
//            }
//        }
//    }
//
//    /**
//     * 释放资源
//     */
//    public void close() {
//        if (net != null) {
//            // net.release();  // 如果不再需要，释放网络资源
//        }
//    }
//
//    /**
//     * 主函数
//     * @param args
//     */
//    public static void main(String[] args) {
//
//        System.setProperty("OPENCV_VIDEOIO_PRIORITY_MSMF", "1"); // 优先使用Media Foundation
//        System.setProperty("OPENCV_LOG_LEVEL", "ERROR"); // 日志级别设为ERROR
//
//        try {
//
//            // 创建PoseControlSystem对象, 传入模型文件和配置文件(常见的 OpenPose 模型)
//            PoseControlSystem poseControl = new PoseControlSystem(
//                    "D:\\VR\\opencv\\openpose_caffe_models\\caffe_models\\pose\\coco\\pose_iter_440000.caffemodel",
//                    "D:\\VR\\opencv\\openpose_caffe_models\\caffe_models\\pose\\coco\\pose_deploy_linevec.prototxt");
//
//            // 打开摄像头
//            VideoCapture capture = openCamera();
//
//            // 创建Mat对象
//            Mat frame = new Mat();
//
//            // 循环处理视频流
//            while (true) {
//                // 捕获一帧视频
//                if (!capture.read(frame)) {
//                    System.err.println("帧捕获失败");
//                    break;
//                }
//                // 检查帧是否为空
//                if (frame.empty()) continue;
//
//                // 处理帧
//                processFrame(poseControl, frame);
//            }
//            // 释放资源
//            releaseResources(capture, poseControl, frame);
//        } catch (Exception e) {
//            e.printStackTrace();
//        }
//    }
//
//    /**
//     * 处理帧
//     * @param poseControl PoseControlSystem对象
//     * @param frame Mat对象
//     */
//    private static void processFrame(PoseControlSystem poseControl, Mat frame) {
//        if (frame.empty()) {
//            System.err.println("捕获的帧为空");
//            return;
//        }
//
//        Mat displayFrame = new Mat();// 创建一个与输入Mat对象大小相同的Mat对象
//        try {
//            frame.copyTo(displayFrame); // 复制输入Mat对象到输出Mat对象
//            List<Point> points = poseControl.detectPose(displayFrame); // 检测关键点
//            poseControl.controlBasedOnPose(points); // 控制
//            drawLandmarks(displayFrame, points); // 绘制关键点
//
//            if (displayFrame.size().width > 0 && displayFrame.size().height > 0) { // 确保Mat对象不为空
//                HighGui.imshow("姿态控制", displayFrame);
//            }
//
//        } finally {
//            if (!displayFrame.empty()) {
//                displayFrame.release(); // 确保释放资源
//            }
//        }
//    }
//
//    /**
//     * 绘制关键点
//     * @param frame Mat对象
//     * @param points 关键点列表
//     */
//    private static void drawLandmarks(Mat frame, List<Point> points) {
//        for (Point p : points) {
//            if (p != null) {
//                Imgproc.circle(frame, p, 5, new Scalar(0, 255, 0), -1);
//            }
//        }
//    }
//
//    /**
//     * 打开摄像头
//     * @return
//     */
//    private static VideoCapture openCamera() {
//        VideoCapture capture = new VideoCapture();
//        if (!capture.open(0, Videoio.CAP_MSMF)) {
//            if (!capture.open(0, Videoio.CAP_DSHOW)) {
//                if (!capture.open(0, Videoio.CAP_ANY)) {
//                    throw new RuntimeException("无法打开摄像头");
//                }
//            }
//        }
//        capture.set(Videoio.CAP_PROP_FRAME_WIDTH, 1280);    // 设置分辨率
//        capture.set(Videoio.CAP_PROP_FRAME_HEIGHT, 720); // 设置分辨率
//        capture.set(Videoio.CAP_PROP_FPS, 30); // 设置帧率
//        capture.set(Videoio.CAP_PROP_AUTOFOCUS, 0); // 关闭自动对焦
//        return capture;
//    }
//
//    /**
//     * 释放资源
//     * @param capture  视频捕获对象
//     * @param poseControl  PoseControlSystem对象
//     * @param frame  Mat对象
//     */
//    private static void releaseResources(VideoCapture capture, PoseControlSystem poseControl, Mat frame) {
//        capture.release();
//        poseControl.close();
//        frame.release();
//        HighGui.destroyAllWindows();
//    }
//}
